#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     SMUX_1,              sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     fr,            tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     br,            tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     fl,            tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     bl,            tmotorNormal, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Autonomous Mode Code Template
//
// This file contains a template for simplified creation of an autonomous program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "drivers/CompleteDriver.h"

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
//   1. Move motors and servos to a preset position.
//   2. Some sensor types take a short while to reach stable values during which time it is best that
//      robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
//      "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  wait1Msec(50);
  HTMAGstartCal(MAGPOS);
  bDisplayDiagnostics = false;
  eraseDisplay();
  decidepgm();
  StartTask(sensorRead);
  startkalmin();
  bDisplayDiagnostics = true;
  kalmindebugdisplay = false;
  eraseDisplay();
  wait1Msec(50);
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

task main() {
  initializeRobot();
  waitForStart(); // Wait for the beginning of autonomous phase.
  wait1Msec(waittime); // incase we are waiting to avoid collision with our ally
  offramp();
  if(path ==1){
    if (color == REDCL){
      if (side == LEFTSD){
        moveDAS (90, LRADJUST, 50, true); // GO TO Right if on Left. Both sides are now synchronized.
      }
      moveDAS(90,SIDESTEP,50,true);
      moveDAS(0,FORWARDTOCRATES,70, true); // BEGIN PATH
      moveDAS(270, PREPTOHIT, 50, true);
      moveDAS(180, HITTIN, 70,true);
      moveDAS(0, PARKFRONT,70,true);
    }
    else {
      if (side == RIGHTSD){
        moveDAS (270, LRADJUST, 50, true); // GO TO Right if on Left. Both sides are now synchronized.
      }
      moveDAS(270,SIDESTEP,50,true);
      moveDAS(0,FORWARDTOCRATES,70, true); // BEGIN PATH
      moveDAS(90, PREPTOHIT, 50, true);
      moveDAS(180, HITTIN, 70,true);
      moveDAS(0, PARKFRONT,70,true);

    }
  }
  if (path == 2){
    if (color==REDCL){
      if (side == RIGHTSD){
        moveDAS(270,LRADJUST,50,true);
      }
      moveDAS (270, BBSETUP, 70,true);
      rotate(40);
      wait1Msec(500);
      while (heading > 240){}
      moveDAS(0,BBCROSS, 70, false);
    }
    else {
      if (side == LEFTSD){
        moveDAS(90,LRADJUST,50,true);
      }
      moveDAS (90, BBSETUP, 70,true);
      rotate(-40);
      wait1Msec(500);
      while (heading < 120){}
      moveDAS(0,BBCROSS, 70, false);

    }
  }
  if (path == 3){
    if (side == RIGHTSD){
      moveDAS(280, LRADJUST/2 , 50, true);
    }
    if (side == LEFTSD){
      moveDAS(90, LRADJUST/2 , 50, true);
    }
    moveDAS(0,FIRSTHIT,90,true);
    moveDAS(305,SECONDHIT,70, true);
    moveDAS(270, THIRDHIT,70, true);
    moveDAS(45,WOGFINISH,70, true);
  }
}
